#!/usr/bin/python
# -*- coding: UTF-8 -*-
import sys;
import os
sys.path.append("..")
sys.path.append("../../")
from lib.BwRobotLib import BwRobotLib
from RobotCar.CarRobot import CarRobot
from RobotManipulator.ManipulatorRobot import ManipulatorRobot
import keyboard
import time


robotlib = BwRobotLib()
carRobot = CarRobot(robotlib)
manipulator = ManipulatorRobot(robotlib)

#robotlib.mclib.SetIsDebug(True)

#连接机器人设备
robotlib.connectRobot("192.168.3.46",55001)
#robotlib.connectRobot("10.10.100.254",55000)

#配置轮式模式
print("请选择轮子模式 :")
print("2 : 两轮")
print("3 : 三轮")
print("4 : 四向轮")
print("5 : 四麦克朗姆轮")
print("6 : 四并橡胶轮")
mode = robotlib.inputNum()


#加载配置
robotlib.module_ids=robotlib.scanNConfigCarIds()

#配置机器人
#配置ID:ids F(4),B(11),l1(8),l2(8),l3(10),l4(11),r1(4),r2(7),r3(6),r4(6) 【TOP:9】
carRobot.bindIds(robotlib.module_ids,mode)

#加载配置（兼容VS项目启动方式及文件直接启动方式）
import os,inspect
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"
robotlib.loadConfig(cfile)


#配置操作臂
manipulator.bindIds(robotlib.module_ids)

#读取当前坐标
manipulator.readAngles()

#-----------------------------------------------------------------------------
#小车操作臂
#-----------------------------------------------------------------------------


######################################################################
#小车
######################################################################

#处理小车控制指令
def handleCarControlKeys(key):
    isDown = key.event_type == 'down'
    if not isDown:
        carRobot.stop()
        return

    cmd = key.name
    if not(cmd == '8' or cmd == '3'):
        if(carRobot.is_running):
            return

    if(cmd == '1'):
        carRobot.goForward()
    if(cmd == '2'):
        carRobot.goBack()
    if(cmd == '3'):
        carRobot.stop_wheel()
    if(cmd == '4'):
        carRobot.turnLeft()
    if(cmd == '5'):
        carRobot.turnRight()
    if(cmd == '6'):
        carRobot.runLeft()
    if(cmd == '7'):
        carRobot.runRight()
    pass



######################################################################
#操作臂
######################################################################

#控制空间坐标
def controlCoordinate(key):
    cmd = key.name
    #print(cmd)
    #X
    if(cmd == 'i'):
        val = manipulator.spatial_coordinate[0] + 1
        manipulator.spatial_coordinate[0] = val
    if(cmd == 'k'):
        val = manipulator.spatial_coordinate[0] - 1
        manipulator.spatial_coordinate[0] = val

    #Y
    if(cmd == 'o'):
        val = manipulator.spatial_coordinate[1] + 1
        manipulator.spatial_coordinate[1] = val
    if(cmd == 'l'):
        val = manipulator.spatial_coordinate[1] - 1
        manipulator.spatial_coordinate[1] = val

    #Z
    if(cmd == 'p'):
        val = manipulator.spatial_coordinate[2] + 1
        manipulator.spatial_coordinate[2] = val
    if(cmd == 'm'):
        val = manipulator.spatial_coordinate[2] - 1
        manipulator.spatial_coordinate[2] = val

    #Roll
    if(cmd == 'z'):
        val = manipulator.spatial_coordinate[3] + 1
        manipulator.spatial_coordinate[3] = val
    if(cmd == 'x'):
        val = manipulator.spatial_coordinate[3] - 1
        manipulator.spatial_coordinate[3] = val

    #Pitch
    if(cmd == 'c'):
        val = manipulator.spatial_coordinate[4] + 1
        manipulator.spatial_coordinate[4] = val
    if(cmd == 'v'):
        val = manipulator.spatial_coordinate[4] - 1
        manipulator.spatial_coordinate[4] = val

     #Yaw
    if(cmd == 'b'):
        val = manipulator.spatial_coordinate[5] + 1
        manipulator.spatial_coordinate[5] = val
    if(cmd == 'n'):
        val = manipulator.spatial_coordinate[5] - 1
        manipulator.spatial_coordinate[5] = val

    manipulator.pushSpatialCoordinate()
    pass

#控制关节角度
def controlAngles(key):
    cmd = key.name
    #print(cmd)
    #I
    if(cmd == 'q'):
        val = manipulator.position_angles[0] + 1
        manipulator.position_angles[0] = robotlib.fixValue(val,180,-180)
    if(cmd == 'a'):
        val = manipulator.position_angles[0] - 1
        manipulator.position_angles[0] = robotlib.fixValue(val,180,-180)

    #T
    if(cmd == 'w'):
        val = manipulator.position_angles[1] + 1
        manipulator.position_angles[1] = robotlib.fixValue(val,90,-90)
    if(cmd == 's'):
        val = manipulator.position_angles[1] - 1
        manipulator.position_angles[1] = robotlib.fixValue(val,90,-90)

    #T
    if(cmd == 'e'):
        val = manipulator.position_angles[2] + 1
        manipulator.position_angles[2] = robotlib.fixValue(val,90,-90)
    if(cmd == 'd'):
        val = manipulator.position_angles[2] - 1
        manipulator.position_angles[2] = robotlib.fixValue(val,90,-90)

    #I
    if(cmd == 'r'):
        val = manipulator.position_angles[3] + 1
        manipulator.position_angles[3] = robotlib.fixValue(val,180,-180)
    if(cmd == 'f'):
        val = manipulator.position_angles[3] - 1
        manipulator.position_angles[3] = robotlib.fixValue(val,180,-180)

    #T
    if(cmd == 't'):
        val = manipulator.position_angles[4] + 1
        manipulator.position_angles[4] = robotlib.fixValue(val,90,-90)
    if(cmd == 'g'):
        val = manipulator.position_angles[4] - 1
        manipulator.position_angles[4] = robotlib.fixValue(val,90,-90)

    max = 180
    min = -180
    if(manipulator.ids_size <= 6):
        max = 90
        min = 0
     #G/I
    if(cmd == 'y'):      
        val = manipulator.position_angles[5] + 1
        manipulator.position_angles[5] = robotlib.fixValue(val,max,min)
    if(cmd == 'h'):
        val = manipulator.position_angles[5] - 1
        manipulator.position_angles[5] = robotlib.fixValue(val,max,min)

    #G
    if(cmd == 'u'):
        if(manipulator.ids_size > 6):        
           val = manipulator.position_angles[6] + 1
           manipulator.position_angles[6] = robotlib.fixValue(val,90,0)
    if(cmd == 'j'):
        if(manipulator.ids_size > 6):   
           val = manipulator.position_angles[6] - 1
           manipulator.position_angles[6] = robotlib.fixValue(val,90,0)

    manipulator.pushJointsAngles()
    pass

#-----------------------------------------------------------------------------
#主体程序
#-----------------------------------------------------------------------------

#处理控制键
def handleCarControlKeys(key):
     cmd = key.name
     #退出监测
     if(cmd == '0'):
        #keyboard.unhook_all()
        return

     if(cmd == 'i' or cmd == 'k' or cmd == 'o' or cmd == 'l' or cmd == 'p' or cmd == 'm' or cmd == 'z' or cmd == 'x' or cmd == 'c' or cmd == 'v' or cmd == 'b' or cmd == 'n'):
        controlCoordinate(key) 

     if(cmd == 'q' or cmd == 'a' or cmd == 'w' or cmd == 's' or cmd == 'e' or cmd == 'd' or cmd == 'r' or cmd == 'f' or cmd == 't' or cmd == 'g' or cmd == 'y' or cmd == 'h' or cmd == 'u' or cmd == 'j'):
        controlAngles(key) 

     if(cmd == '1' or cmd == '2' or cmd == '3' or cmd == '4' or cmd == '5' or cmd == '6' or cmd == '7'):
        HandleCarControlKeys(key) 

#控制指示
def controlTips():
    print("键盘控制角度: 增加 / 减少")
    print("关节 1 : Q/A;")
    print("关节 2 : W/S;")
    print("关节 3 : E/D;")
    print("关节 4 : R/F;")
    print("关节 5 : T/G;")
    print("关节 6 : Y/H;")
    print("关节 7 : U/J;")

    print("空间坐标 X : I/K;")
    print("空间坐标 Y : O/L;")
    print("空间坐标 Z : P/M;")
    print("空间坐标 Roll: Z/X;")
    print("空间坐标 Pitch: C/V;")
    print("空间坐标 Yaw: B/N")

    print("小车移动 ******************")
    print("1 : 向前走")
    print("2 : 向后走")
    print("3 : 停止")
    print("4 : 左转")
    print("5 : 右转")
    print("6 : 左走（5 : 四麦克朗姆轮）")
    print("7 : 右走（5 : 四麦克朗姆轮）")

    print("-----------------------")
    print("0 :退出")
    keyboard.hook(handleControlKeys)
    #参考Python 键盘/鼠标监听及控制：https://www.jianshu.com/p/8e508c6a05ce
    keyboard.wait('0')
    keyboard.unhook_all()
    pass


#--------------------------------
controlTips()
